﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace UI.Application.Reactor.Helper
{
    public class PIDController
    {
        // PID参数
        private double Kp, Ki, Kd;
        // 反馈变量
        private double prevError, integralError;
        // 输出限制
        private double outputMin, outputMax;

        public PIDController(double kp, double ki, double kd, double outputMin, double outputMax)
        {
            this.Kp = kp;
            this.Ki = ki;
            this.Kd = kd;
            this.outputMin = outputMin;
            this.outputMax = outputMax;
            this.prevError = 0;
            this.integralError = 0;
        }

        // 计算PID控制量
        public double Calculate(double target, double feedback, double deltaTime)
        {
            double error = target - feedback;
            double derivativeError = (error - this.prevError) / deltaTime;
            double output = this.Kp * error + this.Ki * this.integralError + this.Kd * derivativeError;
            output = Math.Abs(output);
            // 输出限制
            if (output > this.outputMax)
            {
                output = this.outputMax;
            }
            else if (output < this.outputMin)
            {
                output = this.outputMin;
            }
            // 更新反馈变量
            this.prevError = error;
            this.integralError += error * deltaTime;
            return output;
        }

        // 重置PID控制器
        public void Reset()
        {
            this.prevError = 0;
            this.integralError = 0;
        }
    }
}
